#include "working_state.h"
#include "fsm/arm/fsm.h"
#include "main_controller.h"
#include "masters/arm_controller.h"
using namespace std;
using namespace arm;
using namespace Eigen;
WorkingState::WorkingState()
{

}

/**
 * @brief ForceState::Update
 * @param fsm
 *
 *
 *
 */

void WorkingState::Update(FSM *fsm)
{
//    static int i=0;
//    cout<<i<<" :force state"<<endl;
    //cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" WorkingState::Update "<<endl;
    char master_device=fsm->arm_controller_->GetMasterDeviceIndex();


    switch (fsm->arm_controller_->GetRunningStatus())
    {
    case main_controller::kRCPause:
        fsm->SetState(BaseState::kPauseState);
        break;
    case main_controller::kRCStop:
        fsm->SetState(BaseState::kSleepState);
        break;
    case main_controller::kRCWorking:
    {
        master_device=fsm->arm_controller_->GetMasterDeviceIndex();

        if(master_device<0)
            fsm->SetState(BaseState::kPauseState);
        else
        {
            if(fsm->arm_controller_->GetConnectionStatus()!=main_controller::SocketConnectionStatusType::kConnectionOkay)
                fsm->SetState(BaseState::kPauseState);
            else
            {
                if(false)//switch coeff
                {
                    //fsm->SetState(BaseState::kResetState);
                }
                else
                {
                    char master_device = fsm->arm_controller_->GetMasterDeviceIndex();
                    if(master_device!=MasterIndex::kLeftDeviceIndex && master_device!=MasterIndex::kRightDeviceIndex)
                        return;
                    vector<double> &data=fsm->arm_controller_->main_controller_->master_device_data_[master_device];

                    Eigen::Matrix4d master_pose=Matrix4d::Identity();
                    master_pose(0,3)=data[0];
                    master_pose(1,3)=data[1];
                    master_pose(2,3)=data[2];

                    Matrix3d m;
                    m=Eigen::AngleAxisd(data[3],Eigen::Vector3d::UnitX())* Eigen::AngleAxisd(data[4], Eigen::Vector3d::UnitY())* Eigen::AngleAxisd(data[5], Eigen::Vector3d::UnitZ());
                    master_pose.block(0,0,3,3)=m;
                    fsm->arm_controller_->SendTarget2Slave(master_pose,data[6]);

                    //TODO
                    //normal working
/*
                    Eigen::Matrix4d master_pose=Matrix4d::Identity();
                    double px,py,pz,ox,oy,oz,pg;
                    double m[3][3];
                    //drdGetPositionAndOrientation(&px,&py,&pz,&ox,&oy,&oz,&pg,m,master_device);

                    master_pose(0,3)=px;master_pose(1,3)=py;master_pose(2,3)=pz;

                    for (int i=0;i<3;i++) {
                        for (int j=0;j<3;j++) {
                            master_pose(i,j)=m[i][j];
                        }
                    }
                    pg=-pg/0.0267505;
                    fsm->arm_controller_->SendTarget2Slave(master_pose,pg);//set target
*/


                    //TODO
                    //SET FORCE FEEDBACK
                    //Vector3d master_force=fsm->arm_controller_->GetArmForce(master_pose.block(0,0,3,3));
                    //double finger_force=fsm->arm_controller_->GetFingerForce(pg);
                    //drdSetForceAndTorqueAndGripperForce(master_force[0],master_force[1],master_force[2],0,0,0,finger_force,master_index);
                }
            }
        }
    }
        break;
    default:
        break;
    }

}

void WorkingState::OnEntry(FSM *fsm)
{
    cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" WorkingState::OnEntry"<<endl;
    char master_device=fsm->arm_controller_->GetMasterDeviceIndex();

    master_device=fsm->arm_controller_->GetMasterDeviceIndex();
    if(master_device<0)
    {
        fsm->SetState(BaseState::kPauseState);
        return;
    }

//    dhdSetForceAndGripperForce(0,0,0,0,master_device);
//    drdRegulatePos(false,master_device);
//    drdRegulateGrip(false,master_device);

}

void WorkingState::OnExit(FSM *fsm)
{
    cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<"  WorkingState::OnExit"<<endl;
//    if(fsm->arm_controller_->SetMasterForce(Eigen::Vector3d::Zero()))
//        cout<<"failed to clear feedback force"<<endl;

}
